//这个抖动是因为在控制误差时调整了正负的速度，如果不调就全部设为正的，就是超过了也不管
//这样画出来的误差就会比较大
#include<rclcpp/rclcpp.hpp>
#include<rclcpp_action/rclcpp_action.hpp>
#include<turtlesim/msg/pose.hpp>
#include<geometry_msgs/msg/twist.hpp>
#include <cmath>
#include<memory>
#include<string>
#include<chrono>
#include<thread>

#include"turtle_square/action/rotate_and_move.hpp"
  
//引入时间字面量命名空间，避免时间表达的繁琐
using namespace std::chrono_literals;
using RotateAndMove=turtle_square::action::RotateAndMove;
using GoalHandleRotateAndMove = rclcpp_action::ServerGoalHandle<RotateAndMove>;

class TurtleSquareServer:public rclcpp::Node
{
public:
    explicit TurtleSquareServer(const rclcpp::NodeOptions &options =rclcpp::NodeOptions())
        : Node("turtle_square_server",options),initial_pose_received_(false)
    {   //action服务器
        action_server_=rclcpp_action::create_server<RotateAndMove>(
            this,
            "turtle_rotate_and_move",
            //三个回调函数，目标，取消，接收
            std::bind(&TurtleSquareServer::handle_goal,this,std::placeholders::_1,std::placeholders::_2),
            std::bind(&TurtleSquareServer::handle_cancel,this,std::placeholders::_1),
            std::bind(&TurtleSquareServer::handle_accepted,this,std::placeholders::_1));
            RCLCPP_INFO(this->get_logger(),"action服务端创建，等待请求...");

        //速度命令发布器
        cmd_vel_publisher_=this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel",10);
        //姿态订阅器
        pose_subscription_=this->create_subscription<turtlesim::msg::Pose>(
            "/turtle1/pose",10,
            std::bind(&TurtleSquareServer::pose_callback,this,std::placeholders::_1));
        
        
        
        RCLCPP_INFO(this->get_logger(),"服务端已经启动，等待客户端发布指令...");

    }

private:
    //成员变量声明
    rclcpp_action::Server<RotateAndMove>::SharedPtr action_server_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_publisher_;
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_subscription_;
    //初始位置与当前位置
    turtlesim::msg::Pose::SharedPtr current_pose_;
    turtlesim::msg::Pose::SharedPtr initial_pose_;
    bool initial_pose_received_;

    //执行阶段，使用枚举定义1每个阶段所对应的值，比如rotate阶段=0，move阶段=1，complete阶段=2
    //phase用来存储这个枚举值
    enum Phase { ROTATE_PHASE=0,MOVE_PHASE=1,COMPLETE_PHASE=2};

    //姿态回调
    void pose_callback(const turtlesim::msg::Pose::SharedPtr msg)
    {
        current_pose_=msg;
        if(!initial_pose_received_&&current_pose_){
            initial_pose_=std::make_shared<turtlesim::msg::Pose>(*current_pose_);
            initial_pose_received_=true;
        }
    }

    //回调函数实现
    //处理目标请求
    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID &uuid,//uuid用于生成目标的唯一标识，在后续使用于cancel和feedback中
        std::shared_ptr<const RotateAndMove::Goal> goal)
    {
            RCLCPP_INFO(this->get_logger(),"收到目标请求：角度：%.1f，距离：%.1f,先旋转：%s",
            goal->target_angle*180.0/M_PI,//cmath自带的π
            goal->target_distance,
            goal->rotate_first ?"是":"否");//将rotate1——first这个bool值转化为是/否 输出
            (void)uuid;
            return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }
    //处理任务取消请求
    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleRotateAndMove> goal_handle)
    {
        (void)goal_handle;
        RCLCPP_INFO(this->get_logger(),"收到取消目标请求！");
        stop_turtle();//停止小乌龟运行
        return rclcpp_action::CancelResponse::ACCEPT;//接受取消
    }
    //接收目标回调
    void handle_accepted(const std::shared_ptr<GoalHandleRotateAndMove> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(),"已接受目标，开始执行...");
        std::thread{std::bind(&TurtleSquareServer::execute,this,std::placeholders::_1),goal_handle}.detach();
        //  上述代码等价于：
        // 相当于在新线程中调用：
        //  this->execute(goal_handle);
    }

    void execute(const std::shared_ptr<GoalHandleRotateAndMove> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(),"开始执行目标");

        const auto goal =goal_handle->get_goal();
        auto result=std::make_shared<RotateAndMove::Result>();
        auto feedback=std::make_shared<RotateAndMove::Feedback>();

        rclcpp::Rate loop_rate(10);

        RCLCPP_INFO(this->get_logger(),"正在获取当前位置信息...");
       
        //current_pose_用于接收姿态信息
        while((!current_pose_|| !initial_pose_received_)&&rclcpp::ok()){
            //查看目标是否被取消
            if(goal_handle->is_canceling()){
                RCLCPP_INFO(this->get_logger(),"执行被取消");
                result->success=false;
                result->message="等待姿势数据时被取消";
                goal_handle->canceled(result);
                return;
            }

            loop_rate.sleep();
        }

        if(!current_pose_){
            result->success=false;
            result->message="无法获取当前位置信息";
            goal_handle->abort(result);
            return;
        }
        //每一段移动开始前，将初始位置设为当前的所在的位置
        initial_pose_=std::make_shared<turtlesim::msg::Pose>(*current_pose_);

        if(goal->rotate_first){
            if(!execute_rotation(goal_handle,goal->target_angle,goal->angular_speed,goal->angle_tolerance,feedback))
            {//旋转失败或者被取消了
                result->success=false;
                result->message="旋转阶段失败";
                goal_handle->abort(result);
                return;
            }
            
            stop_turtle();//旋转结束先停止
            initial_pose_ = std::make_shared<turtlesim::msg::Pose>(*current_pose_);//旋转后先重置参考位姿

            if(!execute_movement(goal_handle,goal->target_distance,goal->linear_speed,goal->distance_tolerance,feedback))
            {//移动失败或者被取消了
                result->success=false;
                result->message="移动阶段失败";
                goal_handle->abort(result);
                return;
            }

        }
        else{
                 // 如果先移动，也在移动开始前重置参考点
                initial_pose_ = std::make_shared<turtlesim::msg::Pose>(*current_pose_);

                if(!execute_movement(goal_handle, goal->target_distance, goal->linear_speed, goal->distance_tolerance, feedback)) {
                result->success = false;
                result->message = "移动阶段失败";
                goal_handle->abort(result);
                return;
            }
            if (!execute_rotation(goal_handle, goal->target_angle, goal->angular_speed, goal->angle_tolerance, feedback)) {
                result->success = false;
                result->message = "旋转阶段失败";
                goal_handle->abort(result);
                return;
            }
            stop_turtle();

        }
        

        //任务完成
        stop_turtle();
        result->success=true;
        result->message="任务成功完成";
        result->final_angle=current_pose_->theta;
        result->final_distance=calculate_distance_traveled();

        goal_handle->succeed(result);
        RCLCPP_INFO(this->get_logger(),"目标执行完成");
    }

    bool execute_rotation(const std::shared_ptr<GoalHandleRotateAndMove> goal_handle,
        float target_angle,
        float angular_speed,
        float angle_tolerance,
        std::shared_ptr<RotateAndMove::Feedback> feedback)
        {
            RCLCPP_INFO(this->get_logger(),"开始旋转阶段");
            rclcpp::Rate loop_rate(10);
            float initial_angle=current_pose_->theta;
            float target_absolute_angle=normalize_angle(initial_angle + target_angle);   
            while(rclcpp::ok()){
                //检查取消请求
                if(goal_handle->is_canceling()){
                    RCLCPP_INFO(this->get_logger(),"旋转取消");
                    stop_turtle();
                    return false;
                }
                
                float angle_error=normalize_angle(target_absolute_angle-current_pose_->theta);
                
                feedback->phase=ROTATE_PHASE;
                feedback->current_angle=current_pose_->theta;
                feedback->angle_error=angle_error;
                
                if (fabs(angle_error)>angle_tolerance){
                    if(angle_error>0){
                        feedback->current_angular_velocity=angular_speed*1.0;
                    }else{
                        feedback->current_angular_velocity=angular_speed*(1.0);

                    }
                }
                else{
                    feedback->current_angular_velocity=0.0;//如果小于容差，说明转到位了，角速度设为0
                }

                goal_handle->publish_feedback(feedback);
                 if(fabs(angle_error)<=angle_tolerance){
                    RCLCPP_INFO(this->get_logger(),"旋转已经完成，误差：%.3f",angle_error);
                    stop_turtle();
                    return true;
                }
        
                auto cmd_vel=std::make_unique<geometry_msgs::msg::Twist>();
                cmd_vel->angular.z=angular_speed*((angle_error>0)?1.0:-1.0);
                cmd_vel->linear.x=0.0;
                cmd_vel_publisher_->publish(std::move(cmd_vel));

                loop_rate.sleep();
            
            }
            stop_turtle();
            return false;
           
    
        }

        bool execute_movement(const std::shared_ptr<GoalHandleRotateAndMove> goal_handle,
            float target_distance,float linear_speed,float distance_tolerance,
            std::shared_ptr<RotateAndMove::Feedback> feedback)
        {
            RCLCPP_INFO(this->get_logger(),"开始移动阶段");

            rclcpp::Rate loop_rate(10);

            while(rclcpp::ok()){//检查目标是否被取消了
                if(goal_handle->is_canceling()){
                    RCLCPP_INFO(this->get_logger(),"移动取消");
                    return false;
                }

                float moved_distance=calculate_distance_traveled();
                float distance_error=target_distance-moved_distance;

                //反馈
                feedback->phase=MOVE_PHASE;
                feedback->moved_distance=moved_distance;
                feedback->distance_error=distance_error;
                //返回当前线速度，如果移动到位了，线速度就设为0
                if(fabs(distance_error)>distance_tolerance){
                    feedback->current_linear_velocity=linear_speed*((distance_error)>0 ? 1.0 :(-1.0));
                }
                else{
                    feedback->current_linear_velocity=0.0;
                }
               
                goal_handle->publish_feedback(feedback);

                if(fabs(distance_error)<=distance_tolerance){
                    RCLCPP_INFO(this->get_logger(),"移动完成");
                    return true;
                }

                //发布移动命令
                 auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
                cmd_vel->linear.x = linear_speed * ((distance_error > 0) ? 1.0 : -1.0);
                cmd_vel->angular.z = 0.0;
                cmd_vel_publisher_->publish(std::move(cmd_vel));

                loop_rate.sleep();
            }
            return false;
        }

        //小乌龟停止
        void stop_turtle(){
            auto cmd_vel=std::make_unique<geometry_msgs::msg::Twist>();
            cmd_vel->linear.x=0.0;
            cmd_vel->angular.z=0.0;
            cmd_vel_publisher_->publish(std::move(cmd_vel));
        }

        float calculate_distance_traveled()
        {
            if(!current_pose_||!initial_pose_)return 0;
            return sqrt(pow(current_pose_->x-initial_pose_->x,2)+pow(current_pose_->y-initial_pose_->y,2));
            
        }

        float normalize_angle(float angle)
        {
            while(angle >M_PI)angle-=2.0*M_PI;
            while(angle<-M_PI)angle+=2.0*M_PI;//使用while就可以把角度都跳到-π到π
            return angle;
        }

};

int main(int argc,char** argv){
    rclcpp::init(argc,argv);
    auto node=std::make_shared<TurtleSquareServer>();
    //运行节点
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}